import ManifoldPoint from "./manifold-point";
import CONSTANT from '../../constant';

export default class ManifoldUpdater {
    constructor(manifold) {
        this._manifold = manifold;
        this.numOldPoints = 0;
        this.oldPoints = new Array(CONSTANT.SETTING_MAX_MANIFOLD_POINTS);
        let _g = 0, _g1 = CONSTANT.SETTING_MAX_MANIFOLD_POINTS;
        while(_g < _g1) this.oldPoints[_g++] = new ManifoldPoint();
    }
    removeOutdatedPoints() {
        const CPT=CONSTANT.SETTING_CONTACT_PERSISTENCE_THRESHOLD;
        let index = this._manifold._numPoints,normal=this._manifold.normal;
        while(--index >= 0) {
            let p = this._manifold._points[index];
            let pos1=p.pos1,pos2=p.pos2;
            let diffX = pos1[0] - pos2[0],diffY = pos1[1] - pos2[1],diffZ = pos1[2] - pos2[2];
            let dotN = normal[0] * diffX + normal[1] * diffY + normal[2] * diffZ;
            if(dotN > CPT) {
                this.removeManifoldPoint(index);
                continue;
            }
            diffX += normal[0] * -dotN;diffY += normal[1] * -dotN;diffZ += normal[2] * -dotN;
            if(diffX * diffX + diffY * diffY + diffZ * diffZ > CPT * CPT) {
                this.removeManifoldPoint(index);
            }
        }
    }
    removeManifoldPoint(index) {
        let lastIndex = --this._manifold._numPoints,points=this._manifold._points;
        if(index !== lastIndex) {
            let tmp = points[index];
            points[index] = points[lastIndex];
            points[lastIndex] = tmp;
        }
        let _this = points[lastIndex];
        let localPos1=_this.localPos1,relPos1=_this.relPos1,pos1=_this.pos1,localPos2=_this.localPos1,relPos2=_this.relPos1,pos2=_this.pos1;
        localPos1[0] = localPos1[1] = localPos1[2] = 0;
        localPos2[0] = localPos2[1] = localPos2[2] = 0;
        relPos1[0] = relPos1[1] = relPos1[2] = 0;
        relPos2[0] = relPos2[1] = relPos2[2] = 0;
        pos1[0] = pos1[1] = pos1[2] = 0;
        pos2[0] = pos2[1] = pos2[2] = 0;
        _this._depth = 0;
        let _this1 = _this._impulse.elements;
        _this1[0] = _this1[1] = _this1[2] = _this1[3] = _this1[4] = _this1[5] = _this1[6] = 0;
        _this._warmStarted = false;
        _this._disabled = false;
        _this._id = -1;
    }
    addManifoldPoint(point,_tf1,_tf2) {
        let num = this._manifold._numPoints,tf1=_tf1.elements,tf2=_tf2.elements;
        if(num === CONSTANT.SETTING_MAX_MANIFOLD_POINTS) {
            let targetIndex = this.computeTargetIndex(point,_tf1,_tf2);
            let mp = this._manifold._points[targetIndex];
            let pos1=mp.pos1,relPos1=mp.relPos1,localPos1=mp.localPos1,pos2=mp.pos2,relPos2=mp.relPos2,localPos2=mp.localPos2;
            let v = point.position1.elements;
            pos1[0] = v[0];pos1[1] = v[1];pos1[2] = v[2];
            v = point.position2.elements;
            pos2[0] = v[0];pos2[1] = v[1];pos2[2] = v[2];
            relPos1[0] = pos1[0] - tf1[0];relPos1[1] = pos1[1] - tf1[1];relPos1[2] = pos1[2] - tf1[2];
            relPos2[0] = pos2[0] - tf2[0];relPos2[1] = pos2[1] - tf2[1];relPos2[2] = pos2[2] - tf2[2];
            localPos1[0] = tf1[3] * relPos1[0] + tf1[6] * relPos1[1] + tf1[9] * relPos1[2];
            localPos1[1] = tf1[4] * relPos1[0] + tf1[7] * relPos1[1] + tf1[10] * relPos1[2];
            localPos1[2] = tf1[5] * relPos1[0] + tf1[8] * relPos1[1] + tf1[11] * relPos1[2];
            localPos2[0] = tf2[3] * relPos2[0] + tf2[6] * relPos2[1] + tf2[9] * relPos2[2];
            localPos2[1] = tf2[4] * relPos2[0] + tf2[7] * relPos2[1] + tf2[10] * relPos2[2];
            localPos2[2] = tf2[5] * relPos2[0] + tf2[8] * relPos2[1] + tf2[11] * relPos2[2];
            mp._depth = point.depth;
            let _this1 = mp._impulse.elements;
            _this1[0] = _this1[1] = _this1[2] = _this1[3] = _this1[4] = _this1[5] = _this1[6] = 0;
            mp._id = point.id;
            mp._warmStarted = false;
            mp._disabled = false;
            return;
        }
        let mp = this._manifold._points[num];
        let pos1=mp.pos1,relPos1=mp.relPos1,localPos1=mp.localPos1,pos2=mp.pos2,relPos2=mp.relPos2,localPos2=mp.localPos2;
        let v = point.position1.elements;
        pos1[0] = v[0];pos1[1] = v[1];pos1[2] = v[2];
        v = point.position2.elements;
        pos2[0] = v[0];pos2[1] = v[1];pos2[2] = v[2];
        relPos1[0] = pos1[0] - tf1[0];relPos1[1] = pos1[1] - tf1[1];relPos1[2] = pos1[2] - tf1[2];
        relPos2[0] = pos2[0] - tf2[0];relPos2[1] = pos2[1] - tf2[1];relPos2[2] = pos2[2] - tf2[2];
        localPos1[0] = tf1[3] * relPos1[0] + tf1[6] * relPos1[1] + tf1[9] * relPos1[2];
        localPos1[1] = tf1[4] * relPos1[0] + tf1[7] * relPos1[1] + tf1[10] * relPos1[2];
        localPos1[2] = tf1[5] * relPos1[0] + tf1[8] * relPos1[1] + tf1[11] * relPos1[2];
        localPos2[0] = tf2[3] * relPos2[0] + tf2[6] * relPos2[1] + tf2[9] * relPos2[2];
        localPos2[1] = tf2[4] * relPos2[0] + tf2[7] * relPos2[1] + tf2[10] * relPos2[2];
        localPos2[2] = tf2[5] * relPos2[0] + tf2[8] * relPos2[1] + tf2[11] * relPos2[2];
        mp._depth = point.depth;
        let _this1 = mp._impulse.elements;
        _this1[0] = _this1[1] = _this1[2] = _this1[3] = _this1[4] = _this1[5] = _this1[6] = 0;
        mp._id = point.id;
        mp._warmStarted = false;
        mp._disabled = false;
        this._manifold._numPoints++;
    }
    computeTargetIndex(newPoint,_tf1,_tf2) {
        let tf1=_tf1.elements;
        let p1 = this._manifold._points[0], p2 = this._manifold._points[1], p3 = this._manifold._points[2], p4 = this._manifold._points[3];
        let relPos11=p1.relPos1,relPos21=p2.relPos1,relPos31=p3.relPos1,relPos41=p4.relPos1;
        let maxDepth = p1._depth;
        let maxDepthIndex = 0;
        if(p2._depth > maxDepth) {
            maxDepth = p2._depth;
            maxDepthIndex = 1;
        }
        if(p3._depth > maxDepth) {
            maxDepth = p3._depth;
            maxDepthIndex = 2;
        }
        if(p4._depth > maxDepth) {
            maxDepthIndex = 3;
        }
        let v = newPoint.position1.elements;
        let rp1X = v[0],rp1Y = v[1],rp1Z = v[2];
        rp1X -= tf1[0];rp1Y -= tf1[1];rp1Z -= tf1[2];
        let p1X = relPos21[0], p1Y = relPos21[1], p1Z = relPos21[2];
        let p2X = relPos31[0], p2Y = relPos31[1], p2Z = relPos31[2];
        let p3X = relPos41[0], p3Y = relPos41[1], p3Z = relPos41[2];
        let v12X = p2X - p1X,v12Y = p2Y - p1Y,v12Z = p2Z - p1Z;
        let v34X = rp1X - p3X,v34Y = rp1Y - p3Y,v34Z = rp1Z - p3Z;
        let v13X = p3X - p1X,v13Y = p3Y - p1Y,v13Z = p3Z - p1Z;
        let v24X = rp1X - p2X,v24Y = rp1Y - p2Y,v24Z = rp1Z - p2Z;
        let v14X = rp1X - p1X,v14Y = rp1Y - p1Y,v14Z = rp1Z - p1Z;
        let v23X = p3X - p2X,v23Y = p3Y - p2Y,v23Z = p3Z - p2Z;
        let cross1X = v12Y * v34Z - v12Z * v34Y,cross1Y = v12Z * v34X - v12X * v34Z,cross1Z = v12X * v34Y - v12Y * v34X;
        let cross2X = v13Y * v24Z - v13Z * v24Y,cross2Y = v13Z * v24X - v13X * v24Z,cross2Z = v13X * v24Y - v13Y * v24X;
        let cross3X = v14Y * v23Z - v14Z * v23Y,cross3Y = v14Z * v23X - v14X * v23Z,cross3Z = v14X * v23Y - v14Y * v23X;
        let a1 = cross1X * cross1X + cross1Y * cross1Y + cross1Z * cross1Z;
        let a2 = cross2X * cross2X + cross2Y * cross2Y + cross2Z * cross2Z;
        let a3 = cross3X * cross3X + cross3Y * cross3Y + cross3Z * cross3Z;

        p1X = relPos11[0]; p1Y = relPos11[1]; p1Z = relPos11[2];
        p2X = relPos31[0]; p2Y = relPos31[1]; p2Z = relPos31[2];
        p3X = relPos41[0]; p3Y = relPos41[1]; p3Z = relPos41[2];
        v12X = p2X - p1X;v12Y = p2Y - p1Y;v12Z = p2Z - p1Z;
        v34X = rp1X - p3X;v34Y = rp1Y - p3Y;v34Z = rp1Z - p3Z;
        v13X = p3X - p1X;v13Y = p3Y - p1Y;v13Z = p3Z - p1Z;
        v24X = rp1X - p2X;v24Y = rp1Y - p2Y;v24Z = rp1Z - p2Z;
        v14X = rp1X - p1X;v14Y = rp1Y - p1Y;v14Z = rp1Z - p1Z;
        v23X = p3X - p2X;v23Y = p3Y - p2Y;v23Z = p3Z - p2Z;
        cross1X = v12Y * v34Z - v12Z * v34Y;cross1Y = v12Z * v34X - v12X * v34Z;cross1Z = v12X * v34Y - v12Y * v34X;
        cross2X = v13Y * v24Z - v13Z * v24Y;cross2Y = v13Z * v24X - v13X * v24Z;cross2Z = v13X * v24Y - v13Y * v24X;
        cross3X = v14Y * v23Z - v14Z * v23Y;cross3Y = v14Z * v23X - v14X * v23Z;cross3Z = v14X * v23Y - v14Y * v23X;
        let a11 = cross1X * cross1X + cross1Y * cross1Y + cross1Z * cross1Z;
        let a21 = cross2X * cross2X + cross2Y * cross2Y + cross2Z * cross2Z;
        let a31 = cross3X * cross3X + cross3Y * cross3Y + cross3Z * cross3Z;
        let a22 = a11 > a21 ? a11 > a31 ? a11 : a31 : a21 > a31 ? a21 : a31;

        p1X = relPos11[0]; p1Y = relPos11[1]; p1Z = relPos11[2];
        p2X = relPos21[0]; p2Y = relPos21[1]; p2Z = relPos21[2];
        p3X = relPos41[0]; p3Y = relPos41[1]; p3Z = relPos41[2];
        v12X = p2X - p1X;v12Y = p2Y - p1Y;v12Z = p2Z - p1Z;
        v34X = rp1X - p3X;v34Y = rp1Y - p3Y;v34Z = rp1Z - p3Z;
        v13X = p3X - p1X;v13Y = p3Y - p1Y;v13Z = p3Z - p1Z;
        v24X = rp1X - p2X;v24Y = rp1Y - p2Y;v24Z = rp1Z - p2Z;
        v14X = rp1X - p1X;v14Y = rp1Y - p1Y;v14Z = rp1Z - p1Z;
        v23X = p3X - p2X;v23Y = p3Y - p2Y;v23Z = p3Z - p2Z;
        cross1X = v12Y * v34Z - v12Z * v34Y;cross1Y = v12Z * v34X - v12X * v34Z;cross1Z = v12X * v34Y - v12Y * v34X;
        cross2X = v13Y * v24Z - v13Z * v24Y;cross2Y = v13Z * v24X - v13X * v24Z;cross2Z = v13X * v24Y - v13Y * v24X;
        cross3X = v14Y * v23Z - v14Z * v23Y;cross3Y = v14Z * v23X - v14X * v23Z;cross3Z = v14X * v23Y - v14Y * v23X;
        let a12 = cross1X * cross1X + cross1Y * cross1Y + cross1Z * cross1Z;
        let a23 = cross2X * cross2X + cross2Y * cross2Y + cross2Z * cross2Z;
        let a32 = cross3X * cross3X + cross3Y * cross3Y + cross3Z * cross3Z;
        let a33 = a12 > a23 ? a12 > a32 ? a12 : a32 : a23 > a32 ? a23 : a32;

        p1X = relPos11[0]; p1Y = relPos11[1]; p1Z = relPos11[2];
        p2X = relPos21[0]; p2Y = relPos21[1]; p2Z = relPos21[2];
        p3X = relPos31[0]; p3Y = relPos31[1]; p3Z = relPos31[2];
        v12X = p2X - p1X;v12Y = p2Y - p1Y;v12Z = p2Z - p1Z;
        v34X = rp1X - p3X;v34Y = rp1Y - p3Y;v34Z = rp1Z - p3Z;
        v13X = p3X - p1X;v13Y = p3Y - p1Y;v13Z = p3Z - p1Z;
        v24X = rp1X - p2X;v24Y = rp1Y - p2Y;v24Z = rp1Z - p2Z;
        v14X = rp1X - p1X;v14Y = rp1Y - p1Y;v14Z = rp1Z - p1Z;
        v23X = p3X - p2X;v23Y = p3Y - p2Y;v23Z = p3Z - p2Z;
        cross1X = v12Y * v34Z - v12Z * v34Y;cross1Y = v12Z * v34X - v12X * v34Z;cross1Z = v12X * v34Y - v12Y * v34X;
        cross2X = v13Y * v24Z - v13Z * v24Y;cross2Y = v13Z * v24X - v13X * v24Z;cross2Z = v13X * v24Y - v13Y * v24X;
        cross3X = v14Y * v23Z - v14Z * v23Y;cross3Y = v14Z * v23X - v14X * v23Z;cross3Z = v14X * v23Y - v14Y * v23X;
        let a13 = cross1X * cross1X + cross1Y * cross1Y + cross1Z * cross1Z;
        let a24 = cross2X * cross2X + cross2Y * cross2Y + cross2Z * cross2Z;
        let a34 = cross3X * cross3X + cross3Y * cross3Y + cross3Z * cross3Z;
        let a4 = a13 > a24 ? a13 > a34 ? a13 : a34 : a24 > a34 ? a24 : a34;

        let max = a1 > a2 ? a1 > a3 ? a1 : a3 : a2 > a3 ? a2 : a3;
        let target = 0;
        if(a22 > max && maxDepthIndex !== 1 || maxDepthIndex === 0) {
            max = a22;
            target = 1;
        }
        if(a33 > max && maxDepthIndex !== 2) {
            max = a33;
            target = 2;
        }
        if(a4 > max && maxDepthIndex !== 3) {
            target = 3;
        }
        return target;
    }
    computeRelativePositions(_tf1,_tf2) {
        let tf1=_tf1.elements,tf2=_tf2.elements;
        let num = this._manifold._numPoints;
        let _g = 0;
        while(_g < num) {
            let p = this._manifold._points[_g++];
            let relPos1=p.relPos1,localPos1=p.localPos1,relPos2=p.relPos2,localPos2=p.localPos2;
            relPos1[0] = tf1[3] * localPos1[0] + tf1[4] * localPos1[1] + tf1[5] * localPos1[2];
            relPos1[1] = tf1[6] * localPos1[0] + tf1[7] * localPos1[1] + tf1[8] * localPos1[2];
            relPos1[2] = tf1[9] * localPos1[0] + tf1[10] * localPos1[1] + tf1[11] * localPos1[2];
            relPos2[0] = tf2[3] * localPos2[0] + tf2[4] * localPos2[1] + tf2[5] * localPos2[2];
            relPos2[1] = tf2[6] * localPos2[0] + tf2[7] * localPos2[1] + tf2[8] * localPos2[2];
            relPos2[2] = tf2[9] * localPos2[0] + tf2[10] * localPos2[1] + tf2[11] * localPos2[2];
            p._warmStarted = true;
        }
    }
    findNearestContactPointIndex(target,_tf1,_tf2) {
        let nearestSq = CONSTANT.SETTING_CONTACT_PERSISTENCE_THRESHOLD * CONSTANT.SETTING_CONTACT_PERSISTENCE_THRESHOLD;
        let tf1=_tf1.elements,tf2=_tf2.elements;
        let idx = -1, _g = 0, _g1 = this._manifold._numPoints;
        while(_g < _g1) {
            let i = _g++;
            let mp = this._manifold._points[i];
            let relPos1=mp.relPos1,relPos2=mp.relPos2;
            let v = target.position1.elements, v1 = target.position2.elements;
            let rp1X = v[0],rp1Y = v[1],rp1Z = v[2];
            let rp2X = v1[0],rp2Y = v1[1],rp2Z = v1[2];
            rp1X -= tf1[0];rp1Y -= tf1[1];rp1Z -= tf1[2];
            rp2X -= tf2[0];rp2Y -= tf2[1];rp2Z -= tf2[2];
            let diff1X = relPos1[0] - rp1X,diff1Y = relPos1[1] - rp1Y,diff1Z = relPos1[2] - rp1Z;
            let diff2X = relPos2[0] - rp2X,diff2Y = relPos2[1] - rp2Y,diff2Z = relPos2[2] - rp2Z;
            let sq1 = diff1X * diff1X + diff1Y * diff1Y + diff1Z * diff1Z;
            let sq2 = diff2X * diff2X + diff2Y * diff2Y + diff2Z * diff2Z;
            let d = sq1 < sq2 ? sq1 : sq2;
            if(d < nearestSq) {
                nearestSq = d;
                idx = i;
            }
        }
        return idx;
    }
    totalUpdate(result,_tf1,_tf2) {
        let tf1=_tf1.elements,tf2=_tf2.elements;
        this.numOldPoints = this._manifold._numPoints;
        let _g = 0, _g1 = this.numOldPoints;
        while(_g < _g1) {
            let i = _g++;
            let _this = this.oldPoints[i];
            let tlp1=_this.localPos1,tlp2=_this.localPos2,trp1=_this.relPos1,trp2=_this.localPos2,tp1=_this.pos1,tp2=_this.pos2;
            let cp = this._manifold._points[i];
            let clp1=cp.localPos1,clp2=cp.localPos2,crp1=cp.relPos1,crp2=cp.localPos2,cp1=cp.pos1,cp2=cp.pos2;
            tlp1[0] = clp1[0];tlp1[1] = clp1[1];tlp1[2] = clp1[2];
            tlp2[0] = clp2[0];tlp2[1] = clp2[1];tlp2[2] = clp2[2];
            trp1[0] = crp1[0];trp1[1] = crp1[1];trp1[2] = crp1[2];
            trp2[0] = crp2[0];trp2[1] = crp2[1];trp2[2] = crp2[2];
            tp1[0] = cp1[0];tp1[1] = cp1[1];tp1[2] = cp1[2];
            tp2[0] = cp2[0];tp2[1] = cp2[1];tp2[2] = cp2[2];
            _this._depth = cp._depth;
            for(let i=0;i<7;i++){
                _this._impulse.elements[i]=cp._impulse.elements[i];
            }
            _this._id = cp._id;
            _this._warmStarted = cp._warmStarted;
            _this._disabled = false;
        }
        let num = result.numPoints;
        this._manifold._numPoints = num;
        let _g2 = 0;
        while(_g2 < num) {
            let i = _g2++;
            let p = this._manifold._points[i];
            let pos1=p.pos1,pos2=p.pos2,relPos1=p.relPos1,relPos2=p.relPos2,localPos1=p.localPos1,localPos2=p.localPos2;
            let ref = result.points[i];
            let v = ref.position1.elements, v1 = ref.position2.elements;
            pos1[0] = v[0];pos1[1] = v[1];pos1[2] = v[2];
            pos2[0] = v1[0];pos2[1] = v1[1];pos2[2] = v1[2];
            relPos1[0] = pos1[0] - tf1[0];relPos1[1] = pos1[1] - tf1[1];relPos1[2] = pos1[2] - tf1[2];
            relPos2[0] = pos2[0] - tf2[0];relPos2[1] = pos2[1] - tf2[1];relPos2[2] = pos2[2] - tf2[2];
            localPos1[0] = tf1[3] * relPos1[0] + tf1[6] * relPos1[1] + tf1[9] * relPos1[2];
            localPos1[1] = tf1[4] * relPos1[0] + tf1[7] * relPos1[1] + tf1[10] * relPos1[2];
            localPos1[2] = tf1[5] * relPos1[0] + tf1[8] * relPos1[1] + tf1[11] * relPos1[2];
            localPos2[0] = tf2[3] * relPos2[0] + tf2[6] * relPos2[1] + tf2[9] * relPos2[2];
            localPos2[1] = tf2[4] * relPos2[0] + tf2[7] * relPos2[1] + tf2[10] * relPos2[2];
            localPos2[2] = tf2[5] * relPos2[0] + tf2[8] * relPos2[1] + tf2[11] * relPos2[2];
            p._depth = ref.depth;
            let _this = p._impulse.elements;
            _this[0] = _this[1] = _this[2] = _this[3] = _this[4] = _this[5] = _this[6] = 0;
            p._id = ref.id;
            p._warmStarted = false;
            p._disabled = false;
            let _g = 0, _g1 = this.numOldPoints;
            while(_g < _g1) {
                let ocp = this.oldPoints[_g++];
                if(p._id === ocp._id) {
                    for(let k=0;k<7;k++){
                        _this[k]=ocp._impulse.elements[k];
                    }
                    p._warmStarted = true;
                    break;
                }
            }
        }
    }
    incrementalUpdate(result,_tf1,_tf2) {
        let tf1=_tf1.elements,tf2=_tf2.elements;
        this._manifold._updateDepthsAndPositions(_tf1,_tf2);
        let _g = 0, _g1 = this._manifold._numPoints;
        while(_g < _g1) this._manifold._points[_g++]._warmStarted = true;
        let newPoint = result.points[0];
        let index = this.findNearestContactPointIndex(newPoint,_tf1,_tf2);
        if(index === -1) {
            this.addManifoldPoint(newPoint,_tf1,_tf2);
        } else {
            let cp = this._manifold._points[index];
            let pos1=cp.pos1,pos2=cp.pos2,relPos1=cp.relPos1,relPos2=cp.relPos2,localPos1=cp.localPos1,localPos2=cp.localPos2;
            let v = newPoint.position1.elements, v1 = newPoint.position2.elements;
            pos1[0] = v[0];pos1[1] = v[1];pos1[2] = v[2];
            pos2[0] = v1[0];pos2[1] = v1[1];pos2[2] = v1[2];
            relPos1[0] = pos1[0] - tf1[0];relPos1[1] = pos1[1] - tf1[1];relPos1[2] = pos1[2] - tf1[2];
            relPos2[0] = pos2[0] - tf2[0];relPos2[1] = pos2[1] - tf2[1];relPos2[2] = pos2[2] - tf2[2];
            localPos1[0] = tf1[3] * relPos1[0] + tf1[6] * relPos1[1] + tf1[9] * relPos1[2];
            localPos1[1] = tf1[4] * relPos1[0] + tf1[7] * relPos1[1] + tf1[10] * relPos1[2];
            localPos1[2] = tf1[5] * relPos1[0] + tf1[8] * relPos1[1] + tf1[11] * relPos1[2];
            localPos2[0] = tf2[3] * relPos2[0] + tf2[6] * relPos2[1] + tf2[9] * relPos2[2];
            localPos2[1] = tf2[4] * relPos2[0] + tf2[7] * relPos2[1] + tf2[10] * relPos2[2];
            localPos2[2] = tf2[5] * relPos2[0] + tf2[8] * relPos2[1] + tf2[11] * relPos2[2];
            cp._depth = newPoint.depth;
        }
        this.removeOutdatedPoints();
    }
}